import sys

sys.path.append("../")
from arm_control.angle import ArmAngle
from arm_control.move import ArmMove
import time

# 机械臂功能测试脚本

if __name__=="__main__":
    # 创建 ArmMove and ArmAngle
    arm_move = ArmMove()
    arm_anlge = ArmAngle(base_pos=[0,0,0])
    
    distance_x = 0.0 
    distance_y = 20.0
    distance_z = 3.8
    
    # 抓取点-低
    grab_position = [distance_x, distance_y, distance_z]
    # 抓取点-高
    grab_position_h = [distance_x, distance_y, distance_z+5.0]
    
    # 放置点-低
    release_position = [distance_x, distance_y+5.0, distance_z]
    # 放置点-高
    release_position_h = [distance_x, distance_y+5.0, distance_z+5.0]
    
    # 获取机械臂位于抓取点-高时, 对应的关节电机位置
    steps = arm_anlge.get_steps(*grab_position_h)
    # 机械臂电机执行到对应位置
    arm_move.go_to(*steps)

    # 获取机械臂位于抓取点-低时, 对应的关节电机位置
    steps = arm_anlge.get_steps(*grab_position)
    # 机械臂电机执行到对应位置
    arm_move.go_to(*steps)

    time.sleep(1.0)
    
    # 机械臂抓取
    arm_move.grip()
    
    # 获取机械臂位于放置点-高时, 对应的关节电机位置
    steps = arm_anlge.get_steps(*release_position_h)
    # 机械臂电机执行到对应位置
    arm_move.go_to(*steps)

    # 获取机械臂位于放置点-低时, 对应的关节电机位置
    steps = arm_anlge.get_steps(*release_position)
    # 机械臂电机执行到对应位置
    arm_move.go_to(*steps)

    time.sleep(1.0)
        
    # 机械臂放置
    arm_move.loose()

    # 机械臂归位
    arm_move.reset()

